#include "stdafx.h"
#include "Matrix4.h"


CMatrix4::CMatrix4(void)
{
	CMatrix4::Identity(*this->m_Element);
}


CMatrix4::~CMatrix4(void)
{
}

DString CMatrix4::PrintSelf()
{
	DString str;
	DString elem;
	str = _T("[");
	for (int i=0; i<4; i++)
	{
		str += _T("[");
		for (int j=0; j<4; j++)
		{
			elem.Format(_T("%f, "),m_Element[i][j]);
			str += elem;
		}
		str.TrimRight(_T(','));
		str += _T("],");
	}
	str.TrimRight(_T(','));
	str += _T("]");

	return str;
}

void CMatrix4::InitByVectors(const CVector4 i, const CVector4 j, const CVector4 k, const CVector4 o)
{
	m_Element[0][0] = i.x; m_Element[1][0] = i.y; m_Element[2][0] = i.z; m_Element[3][0] = i.w;
	m_Element[0][1] = j.x; m_Element[1][1] = j.y; m_Element[2][1] = j.z; m_Element[3][1] = j.w;
	m_Element[0][2] = k.x; m_Element[1][2] = k.y; m_Element[2][2] = k.z; m_Element[3][2] = k.w;
	m_Element[0][3] = o.x; m_Element[1][3] = o.y; m_Element[2][3] = o.z; m_Element[3][3] = o.w;
}

void CMatrix4::DeepCopy(double destination[16], const double source[16])
{
	for (int i = 0; i < 16; i++)
	{
		destination[i] = source[i];
	}
}

void CMatrix4::Zero(double elements[16])
{
	for (int i = 0; i < 16; i++)
	{
		elements[i] = 0.0;
	}
}

void CMatrix4::Identity(double elements[16])
{
	elements[0] = elements[5] = elements[10] = elements[15] = 1.0;
	elements[1] = elements[2] = elements[3] = elements[4] =
		elements[6] = elements[7] = elements[8] = elements[9] =
		elements[11] = elements[12] = elements[13] = elements[14] = 0.0;
}

void CMatrix4::Invert(const double inElements[16], double outElements[16])
{
	// inverse( original_matrix, inverse_matrix )
	// calculate the inverse of a 4x4 matrix
	//
	//     -1
	//     A  = ___1__ adjoint A
	//         det A
	//

	// calculate the 4x4 determinent
	// if the determinent is zero,
	// then the inverse matrix is not unique.

	double det = CMatrix4::Determinant(inElements);
	if (det == 0.0)
	{
		return;
	}

	// calculate the adjoint matrix
	CMatrix4::Adjoint(inElements, outElements);

	// scale the adjoint matrix to get the inverse
	for (int i = 0; i < 16; i++)
	{
		outElements[i] /= det;
	}
}

void CMatrix4::Transpose(const double inElements[16], double outElements[16])
{
	for (int i = 0; i < 4; i++)
	{
		for(int j = i; j < 4; j++)
		{
			double temp = inElements[4*i + j];
			outElements[4*i + j] = inElements[4*j + i];
			outElements[4*j + i] = temp;
		}
	}
}

void CMatrix4::MultiplyPoint(const double elements[16], const double in[4], double out[4])
{
	double v1 = in[0];
	double v2 = in[1];
	double v3 = in[2];
	double v4 = in[3];

	out[0] = v1*elements[0]  + v2*elements[1]  + v3*elements[2]  + v4*elements[3];
	out[1] = v1*elements[4]  + v2*elements[5]  + v3*elements[6]  + v4*elements[7];
	out[2] = v1*elements[8]  + v2*elements[9]  + v3*elements[10] + v4*elements[11];
	out[3] = v1*elements[12] + v2*elements[13] + v3*elements[14] + v4*elements[15];
}

void CMatrix4::MultiplyPoint(const CVector4& in, CVector4& out) const
{
	double v1 = in.x;
	double v2 = in.y;
	double v3 = in.z;
	double v4 = in.w;

	out.x = v1*m_Element[0][0] + v2*m_Element[0][1] + v3*m_Element[0][2] + v4*m_Element[0][3];
	//if (abs(out.x)<=MPR_EPSILON) out.x = 0.0;
		
	out.y = v1*m_Element[1][0] + v2*m_Element[1][1] + v3*m_Element[1][2] + v4*m_Element[1][3];
	//if (abs(out.y)<=MPR_EPSILON) out.y = 0.0;

	out.z = v1*m_Element[2][0] + v2*m_Element[2][1] + v3*m_Element[2][2] + v4*m_Element[2][3];
	//if (abs(out.z)<=MPR_EPSILON) out.z = 0.0;

	out.w = v1*m_Element[3][0] + v2*m_Element[3][1] + v3*m_Element[3][2] + v4*m_Element[3][3];
	//if (abs(out.w)<=MPR_EPSILON) out.w = 0.0;
}

void CMatrix4::Multiply4x4(const CMatrix4 *a, const CMatrix4 *b, CMatrix4 *c)
{
	CMatrix4::Multiply4x4(*a->m_Element, *b->m_Element, *c->m_Element);
}

void CMatrix4::Multiply4x4(const double a[16], const double b[16], double c[16])
{
	double tmp[16];

	for (int i = 0; i < 16; i += 4)
	{
		for (int j = 0; j < 4; j++)
		{
			tmp[i + j] = a[i + 0] * b[j + 0] +
				a[i + 1] * b[j + 4] +
				a[i + 2] * b[j + 8] +
				a[i + 3] * b[j + 12];

			//if (abs(tmp[i+j])<=DBL_EPSILON)
			//	tmp[i+j] = 0.0;
		}
	}

	for (int k = 0; k < 16; k++)
	{
		c[k] = tmp[k];
	}
}

void CMatrix4::Multiply4x4(const CMatrix4& in, CMatrix4& out) const
{
	CMatrix4::Multiply4x4(*this->m_Element, *in.m_Element, *out.m_Element);
}

void CMatrix4::RotateX(double theta,bool rad/*=false*/)
{
	if (!rad)
		theta *= 0.017453292519943295;
	CMatrix4 mtx;
	CVector4 i,j,k,w;
	i.SetValue(1, 0, 0, 0);
	j.SetValue(0, cos(theta), -sin(theta), 0);
	k.SetValue(0, sin(theta), cos(theta), 0);
	w.SetValue(0, 0, 0, 1);
	mtx.InitByVectors(i, j, k, w);
	CMatrix4::Multiply4x4(&mtx,this,this);
}

void CMatrix4::RotateY(double theta,bool rad/*=false*/)
{
	if (!rad)
		theta *= 0.017453292519943295;
	CMatrix4 mtx;
	CVector4 i,j,k,w;
	i.SetValue(cos(theta), 0, sin(theta), 0);
	j.SetValue(0, 1, 0, 0);
	k.SetValue(-sin(theta), 0, cos(theta), 0);
	w.SetValue(0, 0, 0, 1);
	mtx.InitByVectors(i, j, k, w);

	CMatrix4::Multiply4x4(&mtx,this,this);
}

void CMatrix4::RotateZ(double theta,bool rad/*=false*/)
{
	if (!rad)
		theta *= 0.017453292519943295;
	CMatrix4 mtx;
	CVector4 i,j,k,w;
	i.SetValue(cos(theta), -sin(theta), 0, 0);
	j.SetValue(sin(theta), cos(theta), 0, 0);
	k.SetValue(0, 0, 1, 0);
	w.SetValue(0, 0, 0, 1);
	mtx.InitByVectors(i, j, k, w);

	CMatrix4::Multiply4x4(&mtx,this,this);
}

CMatrix4 CMatrix4::RotateXMatrix(double theta, bool rad/*=false*/)
{
	if (!rad)
		theta *= 0.017453292519943295;

	double cos0 = cos(theta);
	double sin0 = sin(theta);

	CMatrix4 mtx;
	//CVector4 i,j,k,w;
	//i.SetValue(1, 0, 0, 0);
	//j.SetValue(0, cos(theta), sin(theta), 0);
	//k.SetValue(0, -sin(theta), cos(theta), 0);
	//w.SetValue(0, 0, 0, 1);
	//mtx.InitByVectors(i, j, k, w);

	mtx.m_Element[0][0] = 1; mtx.m_Element[0][1] = 0;    mtx.m_Element[0][2] = 0;     mtx.m_Element[0][3] = 0;
	mtx.m_Element[1][0] = 0; mtx.m_Element[1][1] = cos0; mtx.m_Element[1][2] = -sin0; mtx.m_Element[1][3] = 0;
	mtx.m_Element[2][0] = 0; mtx.m_Element[2][1] = sin0; mtx.m_Element[2][2] = cos0;  mtx.m_Element[2][3] = 0;
	mtx.m_Element[3][0] = 0; mtx.m_Element[3][1] = 0;    mtx.m_Element[3][2] = 0;     mtx.m_Element[3][3] = 1;

	return mtx;

}

CMatrix4 CMatrix4::RotateYMatrix(double theta, bool rad/*=false*/)
{
	if (!rad)
		theta *= 0.017453292519943295;

	double cos0 = cos(theta);
	double sin0 = sin(theta);

	CMatrix4 mtx;
	//CVector4 i,j,k,w;
	//i.SetValue(cos(theta), 0, sin(theta), 0);
	//j.SetValue(0, 1, 0, 0);
	//k.SetValue(-sin(theta), 0, cos(theta), 0);
	//w.SetValue(0, 0, 0, 1);
	//mtx.InitByVectors(i, j, k, w);

	mtx.m_Element[0][0] = cos0; mtx.m_Element[0][1] = 0; mtx.m_Element[0][2] = sin0; mtx.m_Element[0][3] = 0;
	mtx.m_Element[1][0] = 0;    mtx.m_Element[1][1] = 1; mtx.m_Element[1][2] = 0;     mtx.m_Element[1][3] = 0;
	mtx.m_Element[2][0] = -sin0; mtx.m_Element[2][1] = 0; mtx.m_Element[2][2] = cos0;  mtx.m_Element[2][3] = 0;
	mtx.m_Element[3][0] = 0;    mtx.m_Element[3][1] = 0; mtx.m_Element[3][2] = 0;     mtx.m_Element[3][3] = 1;

	return mtx;
}

CMatrix4 CMatrix4::RotateZMatrix(double theta, bool rad/*=false*/)
{
	if (!rad)
		theta *= 0.017453292519943295;

	double cos0 = cos(theta);
	double sin0 = sin(theta);

	CMatrix4 mtx;
	CVector4 i,j,k,w;
	//i.SetValue(cos(theta), sin(theta), 0, 0);
	//j.SetValue(-sin(theta), cos(theta), 0, 0);
	//k.SetValue(0, 0, 1, 0);
	//w.SetValue(0, 0, 0, 1);
	//mtx.InitByVectors(i, j, k, w);

	mtx.m_Element[0][0] = cos0; mtx.m_Element[0][1] = -sin0; mtx.m_Element[0][2] = 0; mtx.m_Element[0][3] = 0;
	mtx.m_Element[1][0] = sin0; mtx.m_Element[1][1] = cos0;  mtx.m_Element[1][2] = 0; mtx.m_Element[1][3] = 0;
	mtx.m_Element[2][0] = 0;    mtx.m_Element[2][1] = 0;     mtx.m_Element[2][2] = 1; mtx.m_Element[2][3] = 0;
	mtx.m_Element[3][0] = 0;    mtx.m_Element[3][1] = 0;     mtx.m_Element[3][2] = 0; mtx.m_Element[3][3] = 1;

	return mtx;
}

CMatrix4 CMatrix4::RotateAxisMatrix(CVector4 axis, double theta, bool rad/*=false*/)
{
	if (!rad) theta *= 0.017453292519943295;

	CMatrix4 mtx;
	double cos0, sin0, cos1;
	cos0 = cos(theta);
	sin0 = sin(theta);
	cos1 = 1.0 - cos0;

	axis.Normalize();

	double u = axis.x;
	double v = axis.y;
	double w = axis.z;
	
	mtx.m_Element[0][0] = cos0 + u*u*cos1;
	mtx.m_Element[0][1] = u*v*cos1 + w*sin0;
	mtx.m_Element[0][2] = u*w*cos1 - v*sin0;
	mtx.m_Element[0][3] = 0;

	mtx.m_Element[1][0] = u*v*cos1 - w*sin0;
	mtx.m_Element[1][1] = cos0 + v*v*cos1;
	mtx.m_Element[1][2] = v*w*cos1 + u*sin0;
	mtx.m_Element[1][3] = 0;

	mtx.m_Element[2][0] = u*w*cos1 + v*sin0;
	mtx.m_Element[2][1] = v*w*cos1 - u*sin0;
	mtx.m_Element[2][2] = cos0 + w*w*cos1;
	mtx.m_Element[2][3] = 0;

	mtx.m_Element[3][0] = 0;
	mtx.m_Element[3][1] = 0;
	mtx.m_Element[3][2] = 0;
	mtx.m_Element[3][3] = 1;

	return mtx;
}

void CMatrix4::Adjoint(const double elem[16], double outElem[16])
{
	//
	//   adjoint( original_matrix, inverse_matrix )
	//
	//     calculate the adjoint of a 4x4 matrix
	//
	//      Let  a   denote the minor determinant of matrix A obtained by
	//           ij
	//
	//      deleting the ith row and jth column from A.
	//
	//                    i+j
	//     Let  b   = (-1)    a
	//          ij            ji
	//
	//    The matrix B = (b  ) is the adjoint of A
	//                     ij
	//
	double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;

	// assign to individual variable names to aid
	// selecting correct values

	a1 = elem[0];  b1 = elem[1];  c1 = elem[2];  d1 = elem[3];
	a2 = elem[4];  b2 = elem[5];  c2 = elem[6];  d2 = elem[7];
	a3 = elem[8];  b3 = elem[9];  c3 = elem[10]; d3 = elem[11];
	a4 = elem[12]; b4 = elem[13]; c4 = elem[14]; d4 = elem[15];

	// row column labeling reversed since we transpose rows & columns

	outElem[0]  =
		CMatrix4::Determinant3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4);
	outElem[4]  =
		- CMatrix4::Determinant3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4);
	outElem[8]  =
		CMatrix4::Determinant3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4);
	outElem[12]  =
		- CMatrix4::Determinant3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4);

	outElem[1]  =
		- CMatrix4::Determinant3x3(b1, b3, b4, c1, c3, c4, d1, d3, d4);
	outElem[5]  =
		CMatrix4::Determinant3x3(a1, a3, a4, c1, c3, c4, d1, d3, d4);
	outElem[9]  =
		- CMatrix4::Determinant3x3(a1, a3, a4, b1, b3, b4, d1, d3, d4);
	outElem[13]  =
		CMatrix4::Determinant3x3(a1, a3, a4, b1, b3, b4, c1, c3, c4);

	outElem[2]  =
		CMatrix4::Determinant3x3(b1, b2, b4, c1, c2, c4, d1, d2, d4);
	outElem[6]  =
		- CMatrix4::Determinant3x3(a1, a2, a4, c1, c2, c4, d1, d2, d4);
	outElem[10]  =
		CMatrix4::Determinant3x3(a1, a2, a4, b1, b2, b4, d1, d2, d4);
	outElem[14]  =
		- CMatrix4::Determinant3x3(a1, a2, a4, b1, b2, b4, c1, c2, c4);

	outElem[3]  =
		- CMatrix4::Determinant3x3(b1, b2, b3, c1, c2, c3, d1, d2, d3);
	outElem[7]  =
		CMatrix4::Determinant3x3(a1, a2, a3, c1, c2, c3, d1, d2, d3);
	outElem[11]  =
		- CMatrix4::Determinant3x3(a1, a2, a3, b1, b2, b3, d1, d2, d3);
	outElem[15]  =
		CMatrix4::Determinant3x3(a1, a2, a3, b1, b2, b3, c1, c2, c3);
}

double CMatrix4::Determinant(const double elem[16])
{
	double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;

	// assign to individual variable names to aid selecting
	//  correct elements

	a1 = elem[0];  b1 = elem[1];  c1 = elem[2];  d1 = elem[3];
	a2 = elem[4];  b2 = elem[5];  c2 = elem[6];  d2 = elem[7];
	a3 = elem[8];  b3 = elem[9];  c3 = elem[10]; d3 = elem[11];
	a4 = elem[12]; b4 = elem[13]; c4 = elem[14]; d4 = elem[15];

	return a1 * CMatrix4::Determinant3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4)
		- b1 * CMatrix4::Determinant3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4)
		+ c1 * CMatrix4::Determinant3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4)
		- d1 * CMatrix4::Determinant3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
}

double CMatrix4::Determinant3x3(double a1, double a2, double a3, double b1, double b2, double b3, double c1, double c2, double c3)
{
	return ( a1 * CMatrix4::Determinant2x2( b2, b3, c2, c3 )
		- b1 * CMatrix4::Determinant2x2( a2, a3, c2, c3 )
		+ c1 * CMatrix4::Determinant2x2( a2, a3, b2, b3 ) );
}

double CMatrix4::Determinant2x2(double a, double b, double c, double d)
{
	return (a * d - b * c);
}

void CMatrix4::SetElement(int i, int j, double value)
{
	if (this->m_Element[i][j] != value)
	{
		this->m_Element[i][j] = value;
	}
}
